//
// Created by 郝羽 on 2018/11/8.
//

#ifndef RTC_XSWITCH2_RTP_HISTORY_HPP
#define RTC_XSWITCH2_RTP_HISTORY_HPP

#include <bitset>
#include <cstdint>
#include <iostream>

// MAX 不能大于32767
template <int MAX>
class RtpHistory {



    std::bitset<MAX> seq_bitset;
    uint16_t last_seq = 0;
    int total_seq = 0;

public:
/**
         * 增加一个rtp序号
         *
         * @param seq
         */
    void add_seq(uint16_t seq) {
        // 如果是第一个序号
        if (total_seq == 0) {
            // 设置最高位
            seq_bitset.set(MAX-1);
            last_seq = seq;
            total_seq = 1;
        } else {
            // 不是第一个序号，用uint16计算距离d，这样不用考虑wrap
            // d 取值范围 [0, 65535]
            // 0 ---- MAX ---- 65535-MAX ---- 65535
            //【 接受 】【 重置             】【 接受 】
            uint16_t d = seq - last_seq;
            if (d >= 0 && d < MAX) {
                if (d == 0) {
                    return;
                }
                // 如果d in（0, MAX)， 跳过d-1个序号，置位最高位
                seq_bitset >>= d;
                seq_bitset.set(MAX-1);
                last_seq = seq;
                if (total_seq + d <= MAX) {
                    total_seq += d;
                } else {
                    total_seq = MAX;
                }
            } else if (d > UINT16_MAX-MAX && d <= 65535) {
                // 如果 d in (65535-MAX, 65535]，更新对应的位
                d = last_seq - seq;
                if (total_seq < d+1) {
                    total_seq = d+1;
                }
                seq_bitset.set(MAX-1-d);
            } else {
                // 如果 d in [MAX, 65535-MAX]，序号跳跃
                seq_bitset.reset();
                seq_bitset.set(MAX-1);
                last_seq = seq;
                total_seq = 1;
            }
        }
    }

    /** 查找连续的丢包序号
     *
     * @return 第一个发现的丢失序号和丢失个数，如果没有丢失，返回经过检查的最后的序号
     */
    std::pair<uint16_t, int> search_consecutive_lost(int limit) {
        return search_consecutive_lost(last_seq - MAX + 1, limit);
    };

    /** 查找连续的丢包序号
     *
     * @param seq
     * @return 第一个发现的丢失序号和丢失个数，如果没有丢失，返回经过检查的最后的序号
     */
    std::pair<uint16_t, int> search_consecutive_lost(uint16_t seq, int limit) {
        int distance_from_last = distance(seq, last_seq);

        // distance_from_last 是负的，因为表示在last之前第几位
        // 如果比有效的seq位数还多，限制到有效位数
        if (std::abs(distance_from_last) > (total_seq-1)) {
            distance_from_last = 1-total_seq;
            seq = last_seq - total_seq + 1;
        }

        if (distance_from_last <= -MAX || distance_from_last >= 0) {
            // 超出范围 或者 只有一个序号
            return std::make_pair(last_seq+1, 0);
        }

        int last_bit = MAX-1;
        int check_bit = last_bit + distance_from_last;

        uint16_t first_lost = seq;

        int lost_count = 0;

        // 跳过连续的1
        while (check_bit < MAX && seq_bitset.test(check_bit)) {
            first_lost++;
            check_bit++;
        }

        // 记录连续0的个数
        while (check_bit < MAX && !seq_bitset.test(check_bit)
                && lost_count < limit) {
            lost_count++;
            check_bit++;
        }

        return std::make_pair(first_lost, lost_count);
    };

public:
    static int16_t distance(uint16_t seq, uint16_t ref) {
        return seq - ref;
    }
};


#include <cstdint>
#include <list>

/*
 *
 */
template <
  class VType
  , class DType
  , class EType
  , EType MAX_DISTANCE
  , EType MIN_DISTANCE
  , EType RESET_DIST_HIGH
  , EType RESET_DIST_LOW>
class RingNumber{
private:
    VType value_;
    
public:
    RingNumber() = delete;
    RingNumber(int) = delete;
    
    RingNumber(VType v) : value_(v){}
    RingNumber(unsigned v) : value_((VType)v){}
 
    VType value() const { return value_;}
    
//    RingNumber& operator=(RingNumber other) default;
    RingNumber operator+(EType dist) const {
        VType v = value_ + (DType)dist;
        return RingNumber(v);
    }
    
    bool operator==(const RingNumber& other) const {
        return value_ == other.value_;
    }
    
    bool operator!=(const RingNumber& other) const {
        return value_ != other.value_;
    }
    
    bool operator <(const RingNumber& other) const {
        return distance(other) > 0;
    }
    
    bool operator >(const RingNumber& other) const {
        return distance(other) < 0;
    }
    
    RingNumber operator-(EType dist) const {
        VType v = value_ - (DType)dist;
        return RingNumber(v);
    }
    
    EType distance(const RingNumber& other) const {
        return (DType)(other.value_ - value_);
    }
    
    // self=100, dist=10, [100, 110] -> true
    // self=100, dist=-10, [90, 100] -> true
    bool withinDistance(const RingNumber& other, EType dist) const{
        DType dshort = (DType) dist;
        EType realdist = other.distance(*this);
        if(dshort >= 0){
            return realdist >= 0 && realdist <= dshort;
        }else{
            return realdist >= dshort && realdist <= 0;
        }
    }
    
    bool withinRange(const RingNumber& low, const RingNumber& high) const {
        DType dist = (DType) low.distance(high);
        return dist >= 0 && withinDistance(low, dist);
    }
    
    bool isReset(const RingNumber& next) const {
        DType dist = (DType) distance(next);
        return outOfOrder(dist);
    }
    static bool outOfOrder(DType dist){
        return (dist < RESET_DIST_LOW) || (dist > RESET_DIST_HIGH);
    }
};



#include <string>
#include <sstream>
#include <iostream>
#include <iomanip>

typedef uint16_t RTPSeqType;
typedef int16_t RTPSeqDistance;
static const RTPSeqDistance RTPSEQ_DISTANCE_MAX = INT16_MAX;
static const RTPSeqDistance RTPSEQ_DISTANCE_MIN = INT16_MIN;
static const RTPSeqDistance RTPSEQ_REST_DISTANCE_HIGH = 10000;
static const RTPSeqDistance RTPSEQ_REST_DISTANCE_LOW = -10000;

typedef  RingNumber
< RTPSeqType
, RTPSeqDistance
, RTPSeqDistance
, RTPSEQ_DISTANCE_MAX
, RTPSEQ_DISTANCE_MIN
, RTPSEQ_REST_DISTANCE_HIGH
, RTPSEQ_REST_DISTANCE_LOW
> RTPSeq;
struct RTPSeqCompartor{
    bool operator()(const RTPSeq& lhs, const RTPSeq& rhs) const{
        return lhs.distance(rhs) > 0;
    }
};


class NAckUnit{
public:
    #define MAKE_OR_MASK(len) (0xFFFFFFFFU << len)
    static const RTPSeqDistance MAX_LENGTH = 17;
    static const uint32_t DEFAULT_OR_MASK = MAKE_OR_MASK(MAX_LENGTH);
    friend std::ostream &operator<< (std::ostream& os, const NAckUnit& unit);
private:
    RTPSeq seqBase_;
    RTPSeqDistance offset_ = 0; // if offset_ >= MAX_LENGTH, all packets received
    uint32_t bits_ = 0;
    
public:
    
    
    NAckUnit(): seqBase_(0u){}
    NAckUnit(const RTPSeq& base): seqBase_(base){}
    
    void resetTo(const RTPSeq& newBase){
        seqBase_ = newBase;
        offset_ = 0;
        bits_ = 0;
    }
    
    void resetToNextUnit(){
        resetTo(seqBase_+MAX_LENGTH);
    }
    
    void moveTo(const RTPSeq& newBase){
        RTPSeqDistance dist = seqBase_.distance(newBase);
        this->move(dist);
    }
    
    void move(RTPSeqDistance dist){
        if(dist >= 0){
            bits_ >>= dist;
            if(offset_ > dist){
                offset_ -= dist;
            }else{
                offset_ = 0;
            }
        }else{
            bits_ <<= (-dist);
            offset_ = 0;
        }
        seqBase_ = seqBase_ + dist;
    }
    
    RTPSeqDistance updateOffset(){
        for(; offset_ < MAX_LENGTH; ++offset_){
            if(((bits_ >>  offset_) & 0x1) == 0) {
                break;
            }
        }
        return offset_;
    }
    
    void buildReqWithMask(uint16_t& pid, uint16_t& blp, uint32_t mask){
        pid = (seqBase_+offset_).value();
        blp = (uint16_t)((~(bits_|mask)) >> (offset_+1));
    }
    
    bool buildReq(uint16_t& pid, uint16_t& blp){
        RTPSeqDistance dist = updateOffset();
        if(dist < MAX_LENGTH){
            buildReqWithMask(pid, blp, DEFAULT_OR_MASK);
            return true;
        }
        return false;
    }
    
    bool buildReq(uint16_t& pid, uint16_t& blp, uint32_t len){
        RTPSeqDistance dist = updateOffset();
        if(dist >= 0 && dist < MAX_LENGTH && (uint32_t)dist < len){
            buildReqWithMask(pid, blp, MAKE_OR_MASK(len));
            return true;
        }
        return false;
    }
    
    bool withinAndSet(const RTPSeq& seq) {
        RTPSeqDistance dist = seqBase_.distance(seq);
        if(dist >= 0 && dist < MAX_LENGTH){
            bits_ |= (1 << seqBase_.distance(seq));
            return true;
        }
        return false;
    }
    
    RTPSeqDistance distance(const RTPSeq& seq){
        return seqBase_.distance(seq);
    }
    
};

std::ostream &operator<< (std::ostream& os, const NAckUnit& unit){
    os << "base=" << std::setw(5) << std::right << unit.seqBase_.value()
    << ", offset=" << std::setw(2) << std::right << unit.offset_
    << ", bits=" << std::bitset<17>(unit.bits_);
    return  os;
}

class NAckList{
    RTPSeq nextSeq_;
    NAckUnit lastUnit_;
    std::list<NAckUnit> history_;
    bool startPhase_ = true;
    
public:
    static const RTPSeqDistance MAX_DISTANCE = 15*NAckUnit::MAX_LENGTH;
    typedef std::function<void(uint16_t pid, uint16_t blp)> ExtractFunc;
    
    NAckList():nextSeq_(0u),lastUnit_(nextSeq_){
        
    }
    
    void resetTo(const RTPSeq& seq){
        history_.clear();
        startPhase_ = true;
        nextSeq_ = seq;
        lastUnit_.resetTo(seq);
        lastUnit_.withinAndSet(seq);
    }
    
    void removeExpired(){
        // remove out-of-date units
        for(auto it = history_.begin(); it != history_.end(); ){
            NAckUnit& o = *it;
            if(o.distance(nextSeq_) >= MAX_DISTANCE){
                history_.erase(it++); // out-of-date
            }else{
                ++it;
            }
        }
    }
    
    /*
     * return true, if there are holes in last unit
     */
    bool addSeq(const RTPSeq& seq, ExtractFunc func){
        if(startPhase_){
            if(lastUnit_.distance(nextSeq_) == 0){
                // first seq
                lastUnit_.resetTo(seq);
                nextSeq_ = seq;
            }
        }
        
        RTPSeqDistance distance = lastUnit_.distance(seq);
        if(distance >= MAX_DISTANCE || distance < RTPSEQ_REST_DISTANCE_LOW){
            // out of order, reset
            resetTo(seq);
            distance = lastUnit_.distance(seq);
        }
        
        if(distance < 0){
            // here, seq < lastUnit_.seqBase_
            if(startPhase_){
                RTPSeqDistance len = seq.distance(nextSeq_);
                if(len >= NAckUnit::MAX_LENGTH ){
                    // drop this seq, and end of start phase
                    startPhase_ = false;
                    return false;
                }
                // rewind
                lastUnit_.move(distance);
                lastUnit_.withinAndSet(seq);
                if(distance < -1){
                    if(func){
                        uint16_t pid, blp;
                        lastUnit_.buildReq(pid, blp, -distance);
                        func(pid, blp);
                    }
                    return true;
                }
                return false;
            }
            
            // search history list
            for(auto it = history_.begin(); it != history_.end(); ++it){
                if(it->withinAndSet(seq)){
                    if(it->updateOffset() >= lastUnit_.MAX_LENGTH){
                        history_.erase(it++);
                    }
                    break;
                }
            }
            return false;
        }
        
        // here, seq >= lastUnit_.seqBase_

        if(lastUnit_.withinAndSet(seq)){
            if(lastUnit_.updateOffset() >= lastUnit_.MAX_LENGTH){
                lastUnit_.resetToNextUnit();
                startPhase_ = false;
            }
        }else{
            // here, seq > (length of lastUnit_)
            // push last to history
            
            int num = ((distance) / NAckUnit::MAX_LENGTH);
            for(int n = 0; n < num; ++n){
                history_.emplace_back(lastUnit_);
                lastUnit_.resetToNextUnit();
            }
            lastUnit_.withinAndSet(seq);
            startPhase_ = false;
        }
        
        RTPSeqDistance hole_len = nextSeq_.distance(seq);
        if(hole_len > 0 ){
            // there is hole between nextSeq_ and seq
            // quick nack
            if(func){
                for(RTPSeqDistance d = 0; d < hole_len;){
                    RTPSeqDistance remains = hole_len-d;
                    RTPSeqDistance len = remains <= (NAckUnit::MAX_LENGTH-1) ? remains : (NAckUnit::MAX_LENGTH-1);
                    NAckUnit unit(nextSeq_+d);
                    uint16_t pid, blp;
                    unit.buildReq(pid, blp, len);
                    func(pid, blp);
                    d += len;
                }
            }
            
            nextSeq_ = seq+1;
            this->removeExpired();
            
            return true;
        }
        
        if(hole_len == 0){
            nextSeq_ = nextSeq_ + 1;
            this->removeExpired();
        }
        return false;
    }
    
    void extract(std::function<void(uint16_t& pid, uint16_t& blp)> func){
        uint16_t pid, blp;
        for(auto it = history_.begin(); it != history_.end(); ){
            NAckUnit& o = *it;
            if(!o.buildReq(pid, blp)){
                // recv all packets of unit, remove it
                history_.erase(it++);
            }else{
                func(pid, blp);
                ++it;
            }
        }
        
        if(lastUnit_.buildReq(pid, blp, lastUnit_.distance(nextSeq_))){
            func(pid, blp);
        }
    }
    
    void dump(){
        std::ostringstream oss;
        oss << "NAckList -----" << std::endl;
        int n = 0;
        for(auto& o: history_){
            oss << "  unit[" << std::setw(4) << std::right << n << "]=[" << o << "]" << std::endl;
            ++n;
        }
        oss << "  unit[last]=[" << lastUnit_ << "]" << std::endl;
        std::cout << oss.str() << std::endl;
    }
    
public:
    static void reverseToSeqs(uint16_t pid, uint16_t blp, std::list<uint16_t>& seqs){
        seqs.emplace_back(pid);
        for(uint16_t i = 0; i < 16; ++i){
            if((blp >> i) & 0x1){
                seqs.emplace_back(pid+i+1);
            }
        }
    }
    
    static std::list<uint16_t> reverseToSeqs(uint16_t pid, uint16_t blp){
        std::list<uint16_t> seqs;
        reverseToSeqs(pid, blp, seqs);
        return seqs;
    }
};

#endif //RTC_XSWITCH2_RTP_HISTORY_HPP
